import math
import time

def wrap( l ):
  return [ ( l[i], l[ ( i + 1 ) % len(l) ] ) for i in range( len( l ) ) ]

class Vec2:
  def __init__( self, x = 0, y = 0 ):
    self.x = float( x )
    self.y = float( y )
    
  def __repr__( self ):
    return 'Vec2( %.2f, %.2f )' % ( self.x, self.y )
    
  def __eq__( self, v, eps = 1e-5 ):
    return abs( self.x - v.x ) < eps and abs( self.y - v.y ) < eps
    
  def __neg__( self ):
    return Vec2( -self.x, -self.y )
    
  def __add__( self, v ):
    return Vec2( self.x + v.x, self.y + v.y )
    
  def __sub__( self, v ):
    return Vec2( self.x - v.x, self.y - v.y )
    
  def __mul__( self, v ):
    if isinstance( v, Vec2 ):
      return Vec2( self.x * v.x, self.y * v.y )
    else:
      return Vec2( self.x * v, self.y * v )
      
  def __div__( self, v ):
    if isinstance( v, Vec2 ):
      return Vec2( self.x / v.x, self.y / v.y )
    else:
      return Vec2( self.x / v, self.y / v )
      
  def squaredLength( self ):
    return self.x * self.x + self.y * self.y
    
  def length( self ):
    return math.sqrt( self.squaredLength() )
    
  def normalize( self ):
    return self / self.length()
    
  def dot( self, v ):
    return self.x * v.x + self.y * v.y
    
  def project( self, v ):
    return v * ( self.dot( v ) / v.squaredLength() )
    
  def cross( self, v ):
    return self.x * v.y - self.y * v.x
    
  def perp( self ):
    return Vec2( -self.y, self.x )

  @staticmethod
  def distToLine( a, b, p ):
    return ( p - a ).cross( b - a ) / ( b - a ).length()
    
class PointMass:
  def __init__( self, pos, mass, gravity = Vec2( 0, -5 ), maxVel = 5 ):
    self.pos = pos
    self.mass = mass
    self.gravity = gravity
    self.maxVel = maxVel
    self.vel = Vec2()
    self.force = Vec2()
    
  def __repr__( self ):
    return 'PointMass( %s, %.2f )' % ( self.pos, self.mass )
    
  def applyForce( self, f ):
    self.force += f
    
  def update( self, dt ):
    if self.mass > 0:
      self.vel += ( self.force / self.mass + self.gravity ) * dt
    if self.vel.length() > self.maxVel:
      self.vel = self.vel.normalize() * min( self.vel.length(), self.maxVel )
    self.pos += self.vel * dt
    self.force = Vec2()
    
class Spring:
  def __init__( self, p1, p2, p, d ):
    self.p1 = p1
    self.p2 = p2
    self.p = p
    self.d = d
    self.l = ( self.p1.pos - self.p2.pos ).length()
    
  def __repr__( self ):
    return 'Spring( %.2f )' % ( ( self.p1.pos - self.p2.pos ).length() / self.l )
    
  def update( self, dt ):
    diff = self.p2.pos - self.p1.pos
    normDiff = diff.normalize()
    pf = normDiff * ( ( diff.length() - self.l ) / self.l * self.p )
    df = ( self.p1.vel - self.p2.vel ).project( normDiff ) * -self.d
    f = pf + df
    self.p1.applyForce( f )
    self.p2.applyForce( -f )
    
class Body:
  DEFAULT_P = 100
  DEFAULT_D = 1
  DEFAULT_FRICTION = 0.1

  def __init__( self, pointMasses, springs, tris, p = DEFAULT_P, d = DEFAULT_D, friction = DEFAULT_FRICTION ):
    self.pointMasses = pointMasses
    self.p = p
    self.d = d
    self.friction = friction
    
    if springs:
      self.springs = springs
    else:
      self.springs = []
      for i in range( len( self.pointMasses ) ):
        for j in range( i + 1, len( self.pointMasses ) ):
          self.springs.append( Spring( self.pointMasses[i], self.pointMasses[j], p, d ) )
          
    self.tris = tris or []
          
  @staticmethod
  def circle( pos, r, n = 8, m = 1, p = DEFAULT_P, d = DEFAULT_D ):
    pm = m / ( n + 1. )
    center = PointMass( pos, pm )
    points = []
    for i in range( n ):
      th = float( i ) / float( n ) * math.pi * 2
      points.append( PointMass( pos + Vec2( r * math.cos( th ), r * math.sin( th ) ), pm ) )
    springs = []
    tris = []
    for i in range( n ):
      springs.append( Spring( points[i], points[ ( i + 1 ) % n ], p, d ) )
      springs.append( Spring( points[i], center, p, d ) )
      tris.append( ( 0, ( i + 1 ) % n + 1, i + 1 ) )
    return Body( [ center ] + points, springs, tris )
    
  @staticmethod
  def rectangle( min, max, m = 1, p = DEFAULT_P, d = DEFAULT_D ):
    ds = [ Vec2( 0, 0 ), Vec2( 0, 1 ), Vec2( 1, 1 ), Vec2( 1, 0 ) ]
    points = [ PointMass( min + ( max - min ) * dv, m / 4. ) for dv in ds ]
    return Body( points, None, [ (0,1,2), (0,2,3) ], p = p, d = d )
    
  def getEdges( self ):
    return sum( map( wrap, self.tris ), [] )
    
  def getPerimeter( self ):
    def edgesEqual( e1, e2 ):
      return ( e1[0] == e2[0] and e1[1] == e2[1] ) or ( e1[0] == e2[1] and e1[1] == e2[0] )
      
    def edgeInList( e1, l ):
      return any( [ edgesEqual( e1, e2 ) for e2 in l ] )
      
    perimeter = []
    edges = self.getEdges()
    for i, e in enumerate( edges ):
      if not edgeInList( e, edges[:i] ) and not edgeInList( e, edges[i+1:] ):
        perimeter.append( e )
    return perimeter

  def getBoundingBox( self ):
    x = [ pm.pos.x for pm in self.pointMasses ]
    y = [ pm.pos.y for pm in self.pointMasses ]
    return Vec2( min( x ), min( y ) ), Vec2( max( x ), max( y ) )

  def getCenter( self ):
    return sum( [ pm.pos for pm in self.pointMasses ], Vec2() ) / len( self.pointMasses )

  def getBoundingCircle( self ):
    c = self.getCenter()
    return c, max( [ ( pm.pos - c ).length() for pm in self.pointMasses ] )

  def update( self, dt ):
    for spring in self.springs:
      spring.update( dt )
    for pointMass in self.pointMasses:
      pointMass.update( dt )
      
  def inside( self, p ):
    posList = [ pm.pos for pm in self.pointMasses ]
    for tri in self.tris:
      dists = [ Vec2.distToLine( posList[i], posList[j], p ) for i, j in wrap( tri ) ]
      prods = [ i * j for i, j in wrap( dists ) ]
      if all( [ prod >= 0 for prod in prods ] ):
        return True
    return False
        
  def offsetTo( self, p, dir = None ):
    if self.inside( p ):
      offsets = []
      for edge1, edge2 in self.getPerimeter():
        p1 = self.pointMasses[ edge1 ]
        p2 = self.pointMasses[ edge2 ]
        a = p1.pos
        b = p2.pos
        d = Vec2.distToLine( a, b, p )
        offset = ( b - a ).normalize().perp() * d
        if d > 0 and ( not dir or dir.dot( offset ) > 0 ):
          offsets.append( ( offset, ( p1, p2 ) ) )
      return offsets and min( offsets, key = lambda offset: offset[0].length() )
    
  def collide( self, body ):
    for pm in self.pointMasses:
      res = body.offsetTo( pm.pos, self.getCenter() - body.getCenter() )
      if res:
        offset, points = res
        surfaceVel = sum( [ p.vel for p in points ], Vec2() ) / float( len( points ) )
        relVel = pm.vel - surfaceVel
        
        projVel = relVel.project( offset )
        offsetf = offset * self.p - projVel * self.d

        perpOffset = offset.perp()
        perpVel = relVel.project( perpOffset )
        frictionf = perpVel * -body.friction

        f = offsetf + frictionf
        pm.applyForce( f )
        for point in points:
          point.applyForce( f / float( -len( points ) ) )
          
  def overlaps( self, body ):
    return any( [ self.offsetTo( pm.pos ) for pm in body.pointMasses ] ) or any( [ body.offsetTo( pm.pos ) for pm in self.pointMasses ] )
    
  def applyForce( self, f ):
    df = f / float( len( self.pointMasses ) )
    for pm in self.pointMasses:
      pm.applyForce( df )
    
  @staticmethod
  def test():
    r = Body.rectangle( Vec2(), Vec2( 1, 1 ) )
    assert not r.offsetTo( Vec2( 1.5, 0.5 ) )
    assert Vec2( -0.1, 0 ) == r.offsetTo( Vec2( 0.1, 0.5 ) )[0]
    assert Vec2( 0, 0.1 ) == r.offsetTo( Vec2( 0.5, 0.9 ) )[0]
    assert Vec2( 0, -0.1 ) == r.offsetTo( Vec2( 0.2, 0.1 ) )[0]

    c = Body.circle( Vec2( 0, 0 ), 1 )
    assert c.offsetTo( Vec2( 0, 0 ) )
    assert not c.offsetTo( Vec2( -1, -1 ) )
    assert not c.offsetTo( Vec2( 1, -1 ) )
    assert not c.offsetTo( Vec2( -1, 1 ) )
    assert not c.offsetTo( Vec2( 1, 1 ) )
    
class World:
  def __init__( self, bodies ):
    self.bodies = bodies or []

  def run( self, maxTime = -1, mindt = 0.01 ):
    start = last = time.time()
    self.shutdown = False
    while not self.shutdown and ( maxTime < 0 or last - start < maxTime ):
      now = time.time()
      dt = now - last
      t = now - start
      if mindt < 0 or dt > mindt:
        last += dt

        for i, body1 in enumerate( self.bodies ):
          for j, body2 in enumerate( self.bodies ):
            if i != j:
              body1.collide( body2 )
          body1.update( dt )
          
        print '%.2f' % t, [ body.getCenter() for body in self.bodies ]

if __name__ == '__main__':
  Body.test()
  objects = []
  objects += [ Body.rectangle( Vec2( -100, -10 ), Vec2( 100, 0 ), -1 ) ]
  objects += [ Body.circle( Vec2( i * 0.5, 5 + i * 1.5 ), 0.5, 8 ) for i in range( 3 ) ]
  for object in objects[1:]:
    object.applyForce( Vec2( -100, 0 ) )
  World( objects ).run()
